/**
  ******************************************************************************
  * File Name          : motion_service.c
  * Description        : This file provides code for the configuration
  *                      of the TiMotionService instances.
  ******************************************************************************
  * @attention
  * Created by Chen Lu in 20210606
  *
  ******************************************************************************
  */

#include "stdlib.h"
#include "string.h"
#include "main.h"
#include "axis.h"
#include "motion_service.h"
#include "queue.h"

float KP = 0.1;
float KI = 0.5;
float KD = 0.3;

/**
  * @brief  Open a motion control service.
  *
  * @note   This function creates a motion control service instance.
  *
  * @retval A motion control service instance pointer.
  */
TiMotionService * mcs_open(void)
{
	TiMotionService *mcs = (TiMotionService*) malloc(sizeof(TiMotionService));
	for (int i=0;i<3;i++)
	{
		mcs->axis[i] = axis_open(i);
		mcs->motor[i] = mcs->axis[i]->motor;
	}
	return mcs;
}

/**
  * @brief  Return the object controlled to zero.
  *
  * @note   This function returns the object controlled to zero, by returning axis of which one by one.
  * 		Note that axis[0] in the project does not have a zero adjustment, so it need to be adjusted manually.
  *
  * @param  mcs	The motion control service instance pointer.
  * @retval MOTIONSERVICE status.
  */
uint8_t mcs_returnzero(TiMotionService * mcs)
{
	/**Note:
	 * This step is considered to be necessary, in order to adapt to situation where object is already zero when open.
	 * Meanwhile, it also checks whether the zero adjuster works.
	 */
	if (axis_write(mcs->axis[1], 'P', 150) != AXIS_OK)
	{
		printf("%s\r\n","Motion service return zero write error!");
		return MOTIONSERVICE_ERROR;
	}
	if (axis_write(mcs->axis[2], 'P', 150) != AXIS_OK)
	{
		printf("%s\r\n","Motion service return zero write error!");
		return MOTIONSERVICE_ERROR;
	}
	if (axis_start(mcs->axis[1]) != AXIS_OK)
	{
		printf("%s\r\n","Motion service return zero start error!");
		return MOTIONSERVICE_ERROR;
	}
	if (axis_start(mcs->axis[2]) != AXIS_OK)
	{
		printf("%s\r\n","Motion service return zero start error!");
		return MOTIONSERVICE_ERROR;
	}

	//Rotate forward for a specific angle.
	while (mcs->axis[1]->encoder < (30 / STEP_ANGLE) && mcs->axis[2]->encoder < (30 / STEP_ANGLE) );

	if (axis_stop(mcs->axis[1]) != AXIS_OK)
	{
		printf("%s\r\n","Motion service return zero stop error!");
		return MOTIONSERVICE_ERROR;
	}
	if (axis_stop(mcs->axis[2]) != AXIS_OK)
	{
		printf("%s\r\n","Motion service return zero stop error!");
		return MOTIONSERVICE_ERROR;
	}

	//Start to return to zero.
	for (int i=1;i<3;i++)
		if (axis_returnzero(mcs->axis[i])!=AXIS_OK)
		{
			printf("%s\r\n","Motion service axis return zero error!");
			return MOTIONSERVICE_ERROR;
		}

	//Set axis[0] to zero in software, and need to adjust it to zero physically simultaneously.
	mcs->axis[0]->encoder = 0;
	return MOTIONSERVICE_OK;
}

/**
  * @brief  Close a motion control service.
  *
  * @note   This function closes a motion control service, where axis_returnzero(), axis_stop() and axis_close()
  * 		are processed orderly.
  *
  * @param  mcs	The motion control service instance pointer.
  * @retval MOTIONSERVICE status.
  */
uint8_t mcs_close(TiMotionService * mcs)
{
	if (axis_stop(mcs->axis[0])!=AXIS_OK)
	{
		printf("%s\r\n","Motion service close error!");
		return MOTIONSERVICE_ERROR;
	}
	if (axis_close(mcs->axis[0])!=AXIS_OK)
	{
		printf("%s\r\n","Motion service close error!");
		return MOTIONSERVICE_ERROR;
	}
	for (int i=1;i<3;i++)
	{
		if (axis_returnzero(mcs->axis[i])!=AXIS_OK)
		{
			printf("%s\r\n","Motion service close error!");
			return MOTIONSERVICE_ERROR;
		}
		if (axis_stop(mcs->axis[i])!=AXIS_OK)
		{
			printf("%s\r\n","Motion service close error!");
			return MOTIONSERVICE_ERROR;
		}
		if (axis_close(mcs->axis[i])!=AXIS_OK)
		{
			printf("%s\r\n","Motion service close error!");
			return MOTIONSERVICE_ERROR;
		}
	}
	return MOTIONSERVICE_OK;
}

/**
  * @brief  Interpret a piece of text instruction.
  *
  * @note   This function interprets a piece of text instruction into the response that
  * 		motion service control instance need to make.
  *
  * @param  request		Text instruction need to interpret.
  * @param  length		Length of the text instruction.
  * @param  response	Response motion control service need to make. In this project, it will only be assigned
  * 					for instruction MOV_ROTATE.
  * @param  p_type		Text instruction type.
  * 			@arg MOV_ROTATE:	Rotate object according to speed and angle provided by the instruction.
  * 			@arg MOV_ZERO:		Return the object to zero.
  * 			@arg MOV_STOP:		Stop the object immediately(emergency shut down).
  * 			@arg MOV_START:		Start the object from the state of STOP.
  * 			@arg GET_ANGLE:		Get current three angles of the object.
  * 			@arg OBJ_CLOSE:		Close the object.
  * 			@arg OBJ_OPEN:		Open the object.
  * @retval MOTIONSERVICE status.
  */
uint8_t mcs_interpret(uint8_t * request, uint8_t length, uint8_t * response, uint8_t* p_type)
{
	uint8_t* p;
	if (strstr((char*)request,"mov") != NULL)
	{
		p = (uint8_t*) strstr((char*)request,":") + 1;
		if (strstr((char*)p,"rotate") != NULL)
		{
			*p_type = MOV_ROTATE;
			p = (uint8_t*) strstr((char*)request,"rotate") + 6;
			strcpy((char*)response, (char*)p);
		}
		else if (strstr((char*)p,"zero") != NULL)
			{
				*p_type = MOV_ZERO;
			}
		else if (strstr((char*)p,"stop") != NULL)
			{
				*p_type = MOV_STOP;
			}
		else if (strstr((char*)p,"start") != NULL)
			{
				*p_type = MOV_START;
			}
	}
	else if (strstr((char*)request,"getAngle") != NULL)
	{
		*p_type = GET_ANGLE;
	}
	else if (strstr((char*)request,"objectClose") != NULL)
	{
		*p_type = OBJ_CLOSE;
	}
	else if (strstr((char*)request,"objectOpen") != NULL)
	{
		*p_type = OBJ_CLOSE;
	}
	else
	{
		printf("%s\r\n","Motion service interpret error/Wrong command!");
		return MOTIONSERVICE_ERROR;
	}
	//Set request to zero.
	memset(request,0x00,127);
	return MOTIONSERVICE_OK;
}

/**
  * @brief  Execute PID controlling algorithm.
  *
  * @note   This function executes PID controlling algorithm. In this project, however, there is no feedback process.
  * 		So it's set to do nothing at present.
  *
  * @param  mcs		The motion control service instance pointer.
  * @param  task	Task being executed now.
  * @param  KP		PID proportion coefficient.
  * @param  KI		PID integration coefficient.
  * @param  KD		PID differential coefficient.
  * @retval MOTIONSERVICE status.
  */
uint8_t mcs_PID(TiMotionService * mcs, TiTask * task, float KP, float KI, float KD)
{
	return MOTIONSERVICE_OK;
}
